//
// Created by 13289204862@163.com on 2024/5/9.
//


#include "serial.h"
#include "controller.h"
#include "encoder.h"
#include "foc.h"
#include "usr_config.h"
#include "util.h"
#include <string.h>
#include "usbd_cdc_if.h"


static uint8_t mNodeID = 1;
static uint32_t mRxTick = 0;
static uint32_t mTxTick = 0;
static uint32_t mCanBusErrNew = 0;
static uint32_t mCanBusErrOld = 0;


static void can_error_check(void);

static bool can_tx(SerialFrame_t *tx_frame);

static bool can_rx(SerialFrame_t *rx_frame, uint8_t *data, int len);

static void fill_value(uint8_t idx, uint8_t *data);

static void parse_frame(SerialFrame_t *frame);

static void config_callback(uint8_t *data, bool isSet);

void CAN_set_node_id(uint8_t nodeID)
{
    mNodeID = nodeID;
}

void CAN_comm_loop(void)
{
    // rx heartbeat timeout check
    if (UsrConfig.heartbeat_consumer_ms)
    {
        if (get_ms_since(mRxTick) > UsrConfig.heartbeat_consumer_ms)
        {
            MCT_set_state(IDLE);
        }
    }

    // tx heartbeat timeout check
    if (UsrConfig.heartbeat_producer_ms)
    {
        if (get_ms_since(mTxTick) > UsrConfig.heartbeat_producer_ms)
        {
            // Send heartbeat
            SerialFrame_t tx_frame;
            tx_frame.head = 0xAA;
            tx_frame.id = ID_ECHO_BIT | (mNodeID << 5) | CAN_CMD_HEARTBEAT;
            tx_frame.dlc = 0;
            can_tx(&tx_frame);
        }
    }

    // CAN bus error check
    can_error_check();
}

void CAN_receive_callback(uint8_t *data, int len)
{
    SerialFrame_t rxframe = {0};
    if (can_rx(&rxframe, data, len))
    {
        parse_frame(&rxframe);
    }
}

void CAN_reset_rx_timeout(void)
{
    mRxTick = SystickCount;
}

void CAN_reset_tx_timeout(void)
{
    mTxTick = SystickCount;
}

void CAN_tx_statusword(tMCStatusword statusword)
{
    SerialFrame_t tx_frame;
    tx_frame.head = 0xAA;
    tx_frame.id = ID_ECHO_BIT | (mNodeID << 5) | CAN_CMD_STATUSWORD_REPORT;
    tx_frame.dlc = 0;
    tx_frame.dlc += uint32_to_data(statusword.status.status_code, &tx_frame.data[tx_frame.dlc]);
    tx_frame.dlc += uint32_to_data(statusword.errors.errors_code, &tx_frame.data[tx_frame.dlc]);

    // 对8字节数据进行crc校验
    tx_frame.crc = crc32((uint8_t*)&tx_frame.data[0], sizeof(tx_frame.data));
    can_tx(&tx_frame);
}

void CAN_calib_report(int32_t step, uint8_t *data)
{
    SerialFrame_t tx_frame;
    tx_frame.head = 0xAA;
    tx_frame.id = ID_ECHO_BIT | (mNodeID << 5) | CAN_CMD_CALIB_REPORT;
    tx_frame.dlc = 8;
    int32_to_data(step, &tx_frame.data[0]); // data[0]
    memcpy(&tx_frame.data[4], data, 4); // data[4]

    // 对8字节数据进行crc校验
    tx_frame.crc = crc32((uint8_t*)&tx_frame.data[0], sizeof(tx_frame.data));
    can_tx(&tx_frame);
}

void CAN_anticogging_report(int32_t step, int32_t value)
{
    SerialFrame_t tx_frame;
    tx_frame.head = 0xAA;
    tx_frame.id = ID_ECHO_BIT | (mNodeID << 5) | CAN_CMD_ANTICOGGING_REPORT;
    tx_frame.dlc = 0;
    tx_frame.dlc += int32_to_data(step, &tx_frame.data[tx_frame.dlc]);
    tx_frame.dlc += int32_to_data(value, &tx_frame.data[tx_frame.dlc]);

    // 对8字节数据进行crc校验
    tx_frame.crc = crc32((uint8_t*)&tx_frame.data[0], sizeof(tx_frame.data));
    can_tx(&tx_frame);
}

static void can_error_check(void)
{
}

static bool can_tx(SerialFrame_t *tx_frame)
{
    // TODO: 给串口发送操作上锁

    // lock
    CDC_Transmit_FS((uint8_t *) tx_frame, sizeof(SerialFrame_t));
    // unlock

    CAN_reset_tx_timeout();

    return true;
}

static bool can_rx(SerialFrame_t *rx_frame, uint8_t *data, int len)
{
    if (len == sizeof(SerialFrame_t))
    {
        memcpy(rx_frame, data, len);

        return true;
    }
    else
    {
        return false;
    }
}

static void fill_value(uint8_t idx, uint8_t *data)
{
    switch (idx)
    {
        case DATA_TYPE_TORQUE:
            if (UsrConfig.invert_motor_dir)
            {
                float_to_data(-Foc.i_q_filt * UsrConfig.torque_constant, data);
            }
            else
            {
                float_to_data(+Foc.i_q_filt * UsrConfig.torque_constant, data);
            }
            break;

        case DATA_TYPE_ENCODER_VEL:
            if (UsrConfig.invert_motor_dir)
            {
                float_to_data(-Encoder.vel, data);
            }
            else
            {
                float_to_data(+Encoder.vel, data);
            }
            break;

        case DATA_TYPE_ENCODER_POS:
            if (UsrConfig.invert_motor_dir)
            {
                float_to_data(-Encoder.pos, data);
            }
            else
            {
                float_to_data(+Encoder.pos, data);
            }
            break;

        case DATA_TYPE_IQ_FILT:
            if (UsrConfig.invert_motor_dir)
            {
                float_to_data(-Foc.i_q_filt, data);
            }
            else
            {
                float_to_data(+Foc.i_q_filt, data);
            }
            break;

        case DATA_TYPE_VBUS_FILT:
            float_to_data(Foc.v_bus_filt, data);
            break;

        case DATA_TYPE_IBUS_FILT:
            float_to_data(Foc.i_bus_filt, data);
            break;

        case DATA_TYPE_POWER_FILT:
            float_to_data(Foc.power_filt, data);
            break;

        default:
            break;
    }
}

static void parse_frame(SerialFrame_t *frame)
{
    int ret;
    bool echo = false;
    uint8_t node_id = GET_NODE_ID(frame->id);
    uint8_t cmd = GET_CMD(frame->id);

    // Dir check
    if (IS_ECHO(frame->id))
    {
        return;
    }

    // set echo bit
    frame->id |= ID_ECHO_BIT;

    // Node id check
    if (node_id != mNodeID && node_id != 0x1F)
    {
        //return;
    }

    CAN_reset_rx_timeout();

    switch (cmd)
    {
        case CAN_CMD_MOTOR_DISABLE:
            if (frame->dlc == 0)
            {
                ret = MCT_set_state(IDLE);
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_MOTOR_ENABLE:
            if (frame->dlc == 0)
            {
                ret = MCT_set_state(RUN);
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_SET_TORQUE:
            if (frame->dlc == 4)
            {
                if (UsrConfig.invert_motor_dir)
                {
                    Controller.input_torque_buffer = -data_to_float(&frame->data[0]);
                }
                else
                {
                    Controller.input_torque_buffer = +data_to_float(&frame->data[0]);
                }
                if (!UsrConfig.sync_target_enable)
                {
                    CONTROLLER_sync_callback();
                }
            }
            break;

        case CAN_CMD_SET_VELOCITY:
            if (frame->dlc == 4)
            {
                if (UsrConfig.invert_motor_dir)
                {
                    Controller.input_velocity_buffer = -data_to_float(&frame->data[0]);
                }
                else
                {
                    Controller.input_velocity_buffer = +data_to_float(&frame->data[0]);
                }
                if (!UsrConfig.sync_target_enable)
                {
                    CONTROLLER_sync_callback();
                }
            }
            break;

        case CAN_CMD_SET_POSITION:
            if (frame->dlc == 4)
            {
                if (UsrConfig.invert_motor_dir)
                {
                    Controller.input_position_buffer = -data_to_float(&frame->data[0]);
                }
                else
                {
                    Controller.input_position_buffer = +data_to_float(&frame->data[0]);
                }
                if (!UsrConfig.sync_target_enable)
                {
                    CONTROLLER_sync_callback();
                }
            }
            break;

        case CAN_CMD_CALIB_START:
            if (frame->dlc == 0)
            {
                ret = MCT_set_state(CALIBRATION);
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_CALIB_ABORT:
            if (frame->dlc == 0)
            {
                ret = MCT_set_state(IDLE);
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_ANTICOGGING_START:
            if (frame->dlc == 0)
            {
                ret = MCT_set_state(ANTICOGGING);
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_ANTICOGGING_ABORT:
            if (frame->dlc == 0)
            {
                ret = MCT_set_state(IDLE);
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_SET_HOME:
            if (frame->dlc == 0)
            {
                ret = CONTROLLER_set_home();
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_ERROR_RESET:
            if (frame->dlc == 0)
            {
                MCT_reset_error();
                int32_to_data(0, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_GET_STATUSWORD:
            if (frame->dlc == 0)
            {
                uint32_to_data(StatuswordNew.status.status_code, &frame->data[0]);
                uint32_to_data(StatuswordNew.errors.errors_code, &frame->data[4]);
                frame->dlc = 8;
                echo = true;
            }
            break;

        case CAN_CMD_GET_VALUE_1:
            if (frame->dlc == 1)
            {
                fill_value(frame->data[0], frame->data);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_GET_VALUE_2:
            if (frame->dlc == 1)
            {
                fill_value(frame->data[0], frame->data);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_SET_CONFIG:
            if (frame->dlc == 8)
            {
                config_callback(frame->data, true);
                frame->dlc = 8;
                echo = true;
            }
            break;

        case CAN_CMD_GET_CONFIG:
            if (frame->dlc == 4)
            {
                config_callback(frame->data, false);
                frame->dlc = 8;
                echo = true;
            }
            break;

        case CAN_CMD_SAVE_ALL_CONFIG:
            if (frame->dlc == 0)
            {
                ret = 0;
                ret += USR_CONFIG_save_config();
                ret += USR_CONFIG_save_cogging_map();
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_RESET_ALL_CONFIG:
            if (frame->dlc == 0)
            {
                if (MCT_get_state() == IDLE)
                {
                    USR_CONFIG_set_default_config();
                    USR_CONFIG_set_default_cogging_map();
                    ret = 0;
                }
                else
                {
                    ret = -1;
                }
                int32_to_data(ret, &frame->data[0]);
                frame->dlc = 4;
                echo = true;
            }
            break;

        case CAN_CMD_SYNC:
            if (frame->dlc == 0)
            {
                if (UsrConfig.sync_target_enable)
                {
                    CONTROLLER_sync_callback();
                }
            }
            break;

        case CAN_CMD_GET_FW_VERSION:
            if (frame->dlc == 0)
            {
                int32_to_data(FW_VERSION_MAJOR, &frame->data[0]);
                int32_to_data(FW_VERSION_MINOR, &frame->data[4]);
                frame->dlc = 8;
                echo = true;
            }
            break;

        default:
            break;
    }

    if (echo)
    {
        // 对8字节数据进行crc校验
        frame->crc = crc32((uint8_t*)&frame->data[0], sizeof(frame->data));
        can_tx(frame);
    }
}

static void config_callback(uint8_t *data, bool isSet)
{
    int32_t idx = data_to_int32(data) - 1; // 0~32
    int32_t config_number = sizeof(tUsrConfig) / 4 - 132; // 共33个配置项

    printf("config_callback: idx = %ld\n", idx);

    if (idx >= config_number)
    {
        int32_to_data(-1, &data[0]);
        int32_to_data(0, &data[4]);
        return;
    }

    uint32_t *pConfig = &(((uint32_t *) (&UsrConfig))[idx]);

    if (isSet)
    {
        memcpy(pConfig, &data[4], 4);

        FOC_update_current_ctrl_gain(UsrConfig.current_ctrl_bw);
        CONTROLLER_update_input_pos_filter_gain(UsrConfig.position_filter_bw);
    }
    else
    {
        memcpy(&data[4], pConfig, 4);
    }
}
